#include <pcl/ModelCoefficients.h> //模型参数头文件
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h> //索引提取头文件
#include <pcl/filters/voxel_grid.h> //体素分割头文件
#include <pcl/features/normal_3d.h> 
#include <pcl/kdtree/kdtree.h>  
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
 
using namespace std;
int main()
{
	//读入点云文件
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZ>);

	pcl::PCDReader read;
	read.read("../out.pcd",*cloud);
	cout << "初始点云数量为 ： " << cloud->points.size() << endl;
 
	//降采样
	pcl::VoxelGrid<pcl::PointXYZ> vg; //创建体素滤波对象
	vg.setInputCloud(cloud);
	vg.setLeafSize(0.01f,0.01f,0.01f);
	vg.filter(*cloud_filtered);
	cout << "体素下采样后的点云数量为 ： " << cloud_filtered->points.size() << endl;
	
	//平面模型分割
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);//平面模型coefficients->values[0],coefficients->values[1] ,coefficients->values[2] coefficients->values[3]分别对应模型ax+by+cz+d=0中的a,b,c,d。
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);//保存点云的索引inliers->indices[i],为其中保存的索引

 	pcl::SACSegmentation<pcl::PointXYZ> seg;
	seg.setOptimizeCoefficients(true); //选择 模型参数是否需要优化
	seg.setModelType(pcl::SACMODEL_PLANE); //定义为平面模型
	seg.setMethodType(pcl::SAC_RANSAC); //定义分割方法
	seg.setMaxIterations(50); //定义最大迭代次数
	seg.setDistanceThreshold(0.01); //容差范围0.01m
	
	
	int i = 0;
	int nr_points = (int)cloud_filtered->points.size();
	//提取点云中的所有的平面
	while( cloud_filtered->points.size() > 0.3*nr_points)
	{
		seg.setInputCloud(cloud_filtered); //第一步 将整个简化的的点云进行分割
		seg.segment(*inliers, *coefficients); //分割结果索引保存 系数保存
		if (inliers->indices.size() == 0)
		{
			std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
		}
		pcl::ExtractIndices<pcl::PointXYZ> extract;
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);
		extract.setNegative(false); //如果设为true,可以提取指定index之外的点云，即除了平面之外的点云 
		extract.filter(*cloud_plane);
		//cout<<"平面的点的数量为： "<<cloud_plane->points.size()<<endl;
 
		extract.setNegative(true);
		extract.filter(*cloud_f); //去除平面之后的点云
		cloud_filtered = cloud_f;
	}
	//当while循环结束之后，所有的平面将被剔除出去，
	//cloud_filtered之中 只剩下除了平面的所有的点云数据
 	
	

	//欧式聚类提取
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(cloud_filtered);//cloud_filtered 中只有除了平面的点云数据
 
	std::vector<pcl::PointIndices> cluster_indices;  //创建聚类索引点集
	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; //创建聚类对象
	ec.setClusterTolerance(0.03);  //设置近邻距离的搜索半径为0.03
	ec.setMinClusterSize(100); //聚类需要最少100个点云
	ec.setMaxClusterSize(25000); //聚类最多25000个点云
	ec.setSearchMethod(tree);
	ec.setInputCloud(cloud_filtered);
	ec.extract(cluster_indices);  //从点云中提取聚类，并将点云索引保存在cluster_indices之中
 
	
	//从点云聚类中分割出每一块点云，必须迭代访问点云索引，每次访问一次，就会创建一个新的点云文件，并且将当前聚类的所有的放到点云数据集之中
	pcl::PCDWriter writer;
	int j = 0;
	//遍历整个索引集合cluster_indices 
	for (vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>); //创建空的点云集
		//遍历每个索引集合中的每个点，将每个点存入创建的空的点云集合中
		for (vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++) //为空的点云集中逐个赋对象
		{
			cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
		}
		cloud_cluster->width = cloud_cluster->points.size();
		cloud_cluster->height = 1;
		cloud_cluster->is_dense = true;
		cout<<"该聚类点云中点云的点的数量为 : "<< cloud_cluster->points.size()<<endl;
		std::stringstream ss;
		ss << "cloud_cluster_" << j << ".pcd";
		writer.write<pcl::PointXYZ>(ss.str(), *cloud_cluster, false);
		j++;
	}
	return 0;
}

